#!/usr/bin/env python3
"""
LLM Enhanced Voice Processor - LLM增强语音处理器
让EchoKit Server的LLM来解析复杂语音命令并发送结构化指令
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import json
import math
import time
import threading
from typing import Dict, List, Optional, Tuple, Any
from enum import Enum

class MotionState(Enum):
    """运动状态枚举"""
    IDLE = "idle"
    MOVING = "moving"
    TURNING = "turning"
    STOPPING = "stopping"

class LLMEnhancedVoiceProcessor(Node):
    def __init__(self):
        super().__init__('llm_enhanced_voice_processor')
        
        # 运动状态
        self.motion_state = MotionState.IDLE
        self.current_motion_timer = None
        
        # 创建QoS配置
        self.qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            depth=10
        )
        
        # 创建发布器和订阅器
        self.cmd_vel_publisher = self.create_publisher(
            Twist, '/cmd_vel', self.qos_profile
        )
        
        # 订阅来自EchoKit Server的结构化命令
        self.structured_command_subscriber = self.create_subscription(
            String, '/structured_voice_command', self.structured_command_callback, self.qos_profile
        )
        
        # 订阅原始语音文本（备用）
        self.voice_text_subscriber = self.create_subscription(
            String, '/voice_text', self.voice_callback, self.qos_profile
        )
        
        self.status_publisher = self.create_publisher(
            String, '/voice_processor_status', self.qos_profile
        )
        
        # 运动参数配置
        self.motion_config = {
            'linear_speed': 0.25,      # 线速度 m/s  从0.3改为0.25
            'angular_speed': 0.8,     # 角速度 rad/s 从1.0改为0.8
            'angle_tolerance': 2.0,   # 角度容差（度） 从5.0改为2.0
            'distance_tolerance': 0.05, # 距离容差（米）
        }
        
        self.get_logger().info("LLM Enhanced Voice Processor initialized")
        self.get_logger().info("Waiting for structured commands from EchoKit Server LLM...")
        
    def structured_command_callback(self, msg: String):
        """处理来自EchoKit Server LLM的结构化命令"""
        try:
            # 解析JSON格式的结构化命令
            command_data = json.loads(msg.data)
            self.get_logger().info(f"Received structured command: {command_data}")
            
            # 执行结构化命令
            success = self.execute_structured_command(command_data)
            
            if success:
                self.publish_status(f"Executing: {command_data.get('description', 'Unknown command')}")
            else:
                self.publish_status(f"Failed to execute: {command_data}")
                
        except json.JSONDecodeError as e:
            self.get_logger().error(f"Failed to parse structured command JSON: {e}")
            self.publish_status(f"JSON parse error: {msg.data}")
        except Exception as e:
            self.get_logger().error(f"Error processing structured command: {e}")
            self.publish_status(f"Processing error: {str(e)}")
    
    def execute_structured_command(self, command_data: Dict[str, Any]) -> bool:
        """执行结构化命令"""
        try:
            command_type = command_data.get('type')
            
            if command_type == 'timed_motion':
                return self.execute_timed_motion_command(command_data)
            elif command_type == 'continuous_motion':
                return self.execute_continuous_motion_command(command_data)
            elif command_type == 'stop':
                return self.execute_stop_command()
            else:
                self.get_logger().warn(f"Unknown command type: {command_type}")
                return False
                
        except Exception as e:
            self.get_logger().error(f"Error executing structured command: {e}")
            return False
    
    def execute_timed_motion_command(self, command_data: Dict[str, Any]) -> bool:
        """执行定时运动命令"""
        try:
            # 提取参数
            linear_x = command_data.get('linear', 0.0)
            angular_z = command_data.get('angular', 0.0)
            duration = command_data.get('duration', 0.0)
            description = command_data.get('description', 'Timed motion')
            
            # 验证参数
            if duration <= 0:
                self.get_logger().error(f"Invalid duration: {duration}")
                return False
            
            # 执行定时运动
            self.execute_timed_motion(linear_x, angular_z, duration, description)
            return True
            
        except Exception as e:
            self.get_logger().error(f"Error in timed motion: {e}")
            return False
    
    def execute_continuous_motion_command(self, command_data: Dict[str, Any]) -> bool:
        """执行持续运动命令"""
        try:
            # 停止当前运动
            if self.current_motion_timer:
                self.current_motion_timer.cancel()
                self.current_motion_timer = None
            
            # 提取参数
            linear_x = command_data.get('linear', 0.0)
            angular_z = command_data.get('angular', 0.0)
            description = command_data.get('description', 'Continuous motion')
            
            # 发布运动命令
            twist = Twist()
            twist.linear.x = linear_x
            twist.angular.z = angular_z
            self.cmd_vel_publisher.publish(twist)
            
            self.get_logger().info(f"Executing continuous motion: {description}")
            
            # 更新状态
            if angular_z != 0:
                self.motion_state = MotionState.TURNING
            elif linear_x != 0:
                self.motion_state = MotionState.MOVING
            else:
                self.motion_state = MotionState.IDLE
            
            return True
            
        except Exception as e:
            self.get_logger().error(f"Error in continuous motion: {e}")
            return False
    
    def execute_stop_command(self) -> bool:
        """执行停止命令"""
        try:
            # 停止定时器
            if self.current_motion_timer:
                self.current_motion_timer.cancel()
                self.current_motion_timer = None
            
            # 发布停止命令
            twist = Twist()
            twist.linear.x = 0.0
            twist.angular.z = 0.0
            self.cmd_vel_publisher.publish(twist)
            
            # 更新状态
            self.motion_state = MotionState.IDLE
            
            self.get_logger().info("Executed stop command")
            return True
            
        except Exception as e:
            self.get_logger().error(f"Error in stop command: {e}")
            return False
    
    def execute_timed_motion(self, linear_x: float, angular_z: float, 
                           duration: float, description: str):
        """执行定时运动"""
        # 停止当前运动
        if self.current_motion_timer:
            self.current_motion_timer.cancel()
        
        # 更新状态
        if angular_z != 0:
            self.motion_state = MotionState.TURNING
        else:
            self.motion_state = MotionState.MOVING
        
        # 发布运动命令
        twist = Twist()
        twist.linear.x = linear_x
        twist.angular.z = angular_z
        self.cmd_vel_publisher.publish(twist)
        
        self.get_logger().info(f"Starting motion: {description} for {duration:.2f}s")
        self.get_logger().info(f"Motion parameters: linear_x={linear_x:.3f}, angular_z={angular_z:.3f}")
        
        # 设置定时器自动停止
        self.current_motion_timer = threading.Timer(duration, self.stop_motion)
        self.current_motion_timer.start()
    
    def stop_motion(self):
        """停止运动"""
        # 发布停止命令
        twist = Twist()
        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.cmd_vel_publisher.publish(twist)
        
        # 更新状态
        self.motion_state = MotionState.IDLE
        self.current_motion_timer = None
        
        self.get_logger().info("Motion completed - stopped")
        self.publish_status("Motion completed")
    
    def voice_callback(self, msg: String):
        """处理原始语音输入（备用方案）"""
        voice_text = msg.data.strip()
        self.get_logger().info(f"Received raw voice text: '{voice_text}'")
        
        # 如果没有收到结构化命令，可以在这里做简单的备用处理
        # 或者发送给EchoKit Server请求LLM处理
        self.publish_status(f"Raw voice received: {voice_text}")
    
    def publish_status(self, status: str):
        """发布状态信息"""
        status_msg = String()
        status_msg.data = status
        self.status_publisher.publish(status_msg)
    
    def destroy_node(self):
        """清理资源"""
        if self.current_motion_timer:
            self.current_motion_timer.cancel()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    
    try:
        processor = LLMEnhancedVoiceProcessor()
        rclpy.spin(processor)
    except KeyboardInterrupt:
        pass
    finally:
        if 'processor' in locals():
            processor.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()